#!/usr/bin/env python
import array
import binascii
import math
import roslib; roslib.load_manifest('robobuilder')
import rospy
import serial
import signal
import sys
import threading
import time

import rbc_protocol
import wck_messages
import rb_angles

import robobuilder.msg
from robobuilder.msg import Motion, ServoAngles

MOTION_STOP = 7

ROBOT_IS_IN_DIRECT_CONTROL_MODE = False

SERIAL_PORT = serial.Serial()

PROGRAM_GOING_TO_STOP = False

ROBOT_ACCESS_LOCK = threading.Lock()


def writeMotionMessage(port, motionNum):
	global ROBOT_IS_IN_DIRECT_CONTROL_MODE
	if ROBOT_IS_IN_DIRECT_CONTROL_MODE == True:
		releaseDirectControlMode(port)
	rospy.loginfo(rospy.get_name()+" Executing motion %r", motionNum)
	data = rbc_protocol.getMotionMessage(motionNum)
	port.write(data)
	success = rbc_protocol.validateMessageEcho(port, data)
	#todo do something with success, such as repeating the message

def switchToDirectControlMode(port):
	global ROBOT_IS_IN_DIRECT_CONTROL_MODE
	rospy.loginfo(rospy.get_name()+" Switching to direct control mode.")
	data = rbc_protocol.getDirectControlModeMessage()
	port.write(data)
	success = rbc_protocol.validateMessageEcho(port, data)
	#todo do something with success, such as repeating the message
	ROBOT_IS_IN_DIRECT_CONTROL_MODE = True

def releaseDirectControlMode(port):
	global ROBOT_IS_IN_DIRECT_CONTROL_MODE
	rospy.loginfo(rospy.get_name() + " Releasing direct control mode.")
	data = rbc_protocol.getDirectControlModeReleaseMessage()
	port.write(data)
	ROBOT_IS_IN_DIRECT_CONTROL_MODE = False

def setServoPosRaw(port, servoId, pos):
	global ROBOT_IS_IN_DIRECT_CONTROL_MODE
	if ROBOT_IS_IN_DIRECT_CONTROL_MODE == False:
		switchToDirectControlMode(port)

	data = wck_messages.get10bitPositionMoveMessage(250, servoId, pos)
	port.write(data)
	reply = port.read(2)
	val = (ord(reply[0]) << 7) + (ord(reply[1]) & 0x7f)
	#todo do something with val

def setServoPosRadian(port, servoId, pos):
	raw = rb_angles.convertFromRadianToRaw(pos, servoId)
	setServoPosRaw(port, servoId, raw)	

def setServoPosWithAngleDict(port, aDict):
	for k, v in aDict.iteritems():
		setServoPosRaw(port, k, v)

def motionCallback(data):
	try:
		ROBOT_ACCESS_LOCK.acquire()
		writeMotionMessage(SERIAL_PORT, data.motion)
	finally:
		ROBOT_ACCESS_LOCK.release()

def servoCallback(data):
	try:
		ROBOT_ACCESS_LOCK.acquire()
		if len(data.servo_id) != len(data.servo_angle):
			rospy.logwarn(rospy.get_name() + " Array sizes for servo id and angles were different")
			return
		for i, a in zip(data.servo_id, data.servo_angle):
			setServoPosRadian(SERIAL_PORT, i, a)
	finally:
		ROBOT_ACCESS_LOCK.release()

def publishStatusCallback():
	global ANGLE_POS_PUBLISHER
	global PROGRAM_GOING_TO_STOP
	global PUBLISH_STATUS_TIMER
	try:
		ROBOT_ACCESS_LOCK.acquire()
		data = ServoAngles()
		data.servo_id = range(0,16)
		data.servo_angle = range(0,16)
		for x in data.servo_id:
			pos = readPos(x)
			data.servo_angle[x] = pos

		ANGLE_POS_PUBLISHER.publish(data)

		if PROGRAM_GOING_TO_STOP == False:
			PUBLISH_STATUS_TIMER = threading.Timer(0.1, publishStatusCallback)
			PUBLISH_STATUS_TIMER.start()
	finally:
		ROBOT_ACCESS_LOCK.release()


def connector():
	global ANGLE_POS_PUBLISHER
	rospy.init_node('connector', anonymous=True)
	rospy.Subscriber('robobuilder_motion', Motion, motionCallback)
	rospy.Subscriber('robobuilder_servos', ServoAngles, servoCallback)
	ANGLE_POS_PUBLISHER = rospy.Publisher('/robobuilder_angles', ServoAngles)
	PUBLISH_STATUS_TIMER.start()
	rospy.spin()

def readPos(servoNum):
	global SERIAL_PORT
	if ROBOT_IS_IN_DIRECT_CONTROL_MODE == False:
		switchToDirectControlMode(SERIAL_PORT)
	data = wck_messages.get10bitPositionReadMessage(servoNum)
	SERIAL_PORT.write(data)
	reply = SERIAL_PORT.read(2)
	try:
		val = rb_angles.convertFromRawToRadian( (ord(reply[0]) << 7) + (ord(reply[1]) & 0x7f), servoNum )
		return val
	except IndexError:
		return -1

def playTestMotion():
	while True:
		t = time.time()
		x = math.cos(5*t)
		x = x*100 + 400
		setServoPosRaw(SERIAL_PORT, 10, int(x))
		setServoPosRaw(SERIAL_PORT, 11, int(x))
		setServoPosRaw(SERIAL_PORT, 12, int(x))
		setServoPosRaw(SERIAL_PORT, 13, 1024-int(x))
		setServoPosRaw(SERIAL_PORT, 14, 1024-int(x))
		setServoPosRaw(SERIAL_PORT, 15, 1024-int(x))
		y = -math.cos(5*t)
		y = y*50 + 400
		setServoPosRaw(SERIAL_PORT, 0, int(y))
		setServoPosRaw(SERIAL_PORT, 1, 1024-int(y))
		setServoPosRaw(SERIAL_PORT, 2, 1024-int(y))
		setServoPosRaw(SERIAL_PORT, 3, int(-math.cos(t)*25+375))
		setServoPosRaw(SERIAL_PORT, 4, int(y))

		setServoPosRaw(SERIAL_PORT, 5, 1024-int(y))
		setServoPosRaw(SERIAL_PORT, 6, int(y))
		setServoPosRaw(SERIAL_PORT, 7, int(y))
		setServoPosRaw(SERIAL_PORT, 8, int(-math.cos(t)*25+625))
		setServoPosRaw(SERIAL_PORT, 9, 1024-int(y))

		time.sleep(0.01)

def signal_handler(signal, frame):
	global PROGRAM_GOING_TO_STOP
	global PUBLISH_STATUS_TIMER
	print 'You pressed Ctrl+C!'
	PROGRAM_GOING_TO_STOP = True
	PUBLISH_STATUS_TIMER.cancel()
        sys.exit(0)

if __name__ == '__main__':
	signal.signal(signal.SIGINT, signal_handler)
	SERIAL_PORT = serial.Serial("/dev/ttyUSB0", 115200, timeout=1)
	SERIAL_PORT.open()
	releaseDirectControlMode(SERIAL_PORT)
	PUBLISH_STATUS_TIMER = threading.Timer(0.1, publishStatusCallback)
	
	#print readPos(10)
	#playTestMotion()
	#zeroAngles = rb_angles.getDefaultRawZeroAngleDictionary()
	#setServoPosWithAngleDict(SERIAL_PORT, zeroAngles)
	#time.sleep(1.0)
	#writeMotionMessage(SERIAL_PORT, MOTION_STOP)
        connector()

